import java.util.Stack;

public class KdTree {
	
	private Node2d root;
	private int size;
	
	private class Node2d {
		int depth;
		RectHV rect;
		Point2D p;
		Node2d left;
		Node2d right;
	}
	
	public KdTree() {                              // construct an empty set of points
		root = null;
		size = 0;
	}
	
	public boolean isEmpty() {                        // is the set empty?
		return size == 0;
	}
	
	public int size() {                              // number of points in the set
		return size;
	}
	
	public void insert(Point2D p) {                  // add the point p to the set (if it is not already in the set)
		if (!contains(p))
			root = insertNode(root, p, 0, 0.0, 0.0, 1.0, 1.0);
//			root = insertNode(root, p, 0, new RectHV(0.0, 0.0, 1.0, 1.0));
	}
	
	private Node2d insertNode(Node2d node, Point2D p, int depth, double minx, double miny, double maxx, double maxy) {                  // add the point p to the set (if it is not already in the set)

		if (node == null) {
			node = new Node2d();
			node.rect = new RectHV(minx, miny, maxx, maxy);
			node.depth = depth;
			node.p = p;
			node.left = null;
			node.right = null;
			size++;
		} else {
			if (node.depth % 2 == 0) {
				if (Point2D.X_ORDER.compare(p, node.p) < 0)
					node.left = insertNode(node.left, p, ++depth, minx, miny, node.p.x(), maxy);
				else
					node.right = insertNode(node.right, p, ++depth, node.p.x(), miny, maxx, maxy);
			} else {
				if (Point2D.Y_ORDER.compare(p, node.p) < 0)
					node.left = insertNode(node.left, p, ++depth, minx, miny, maxx, node.p.y());
				else
					node.right = insertNode(node.right, p, ++depth, minx, node.p.y(), maxx, maxy);
			}
		}
		
		return node;
	}
	
//	private Node2d insertNode(Node2d node, Point2D p, int depth, RectHV rect) {                  // add the point p to the set (if it is not already in the set)
//
//		if (node == null) {
//			node = new Node2d();
//			node.rect = rect;
//			node.depth = depth;
//			node.p = p;
//			node.left = null;
//			node.right = null;
//			size++;
//		} else {
//			if (node.depth % 2 == 0) {
//				if (Point2D.X_ORDER.compare(p, node.p) < 0)
//					node.left = insertNode(node.left, p, ++depth, new RectHV(rect.xmin(), rect.ymin(), node.p.x(), rect.ymax()));
//				else
//					node.right = insertNode(node.right, p, ++depth, new RectHV(node.p.x(), rect.ymin(), rect.xmax(), rect.ymax()));
//			} else {
//				if (Point2D.Y_ORDER.compare(p, node.p) < 0)
//					node.left = insertNode(node.left, p, ++depth, new RectHV(rect.xmin(), rect.ymin(), rect.xmax(), node.p.y()));
//				else
//					node.right = insertNode(node.right, p, ++depth, new RectHV(rect.xmin(), node.p.y(), rect.xmax(), rect.ymax()));
//			}
//		}
//		
//		return node;
//	}
//	
		
	public boolean contains(Point2D p) {             // does the set contain the point p?
		return containsNode(root, p);
	}
	
	private boolean containsNode(Node2d node, Point2D p) {
		if (node == null)
			return false;
		
		if (node.p.equals(p))
			return true;
		else
			if (node.depth % 2 == 0) {
				if (Point2D.X_ORDER.compare(p, node.p) < 0)
					return containsNode(node.left, p);
				else
					return containsNode(node.right, p);
			} else {
				if (Point2D.Y_ORDER.compare(p, node.p) < 0)
					return containsNode(node.left, p);
				else
					return containsNode(node.right, p);
			}
	}
	
	public void draw() {                             // draw all of the points to standard draw
		drawNode(root);
	}
	
	private void drawNode(Node2d node) {
        if (node == null)
        	return;
		
		StdDraw.setPenColor(StdDraw.BLACK);
        StdDraw.setPenRadius(.005);
        node.p.draw();
        
    	StdDraw.setPenRadius(.003);
    	
        if (node.depth % 2 == 0) {
        	StdDraw.setPenColor(StdDraw.RED);
        	node.p.drawTo(new Point2D(node.p.x(), node.rect.ymax()));
        	node.p.drawTo(new Point2D(node.p.x(), node.rect.ymin()));
        	
        	drawNode(node.left);
        	drawNode(node.right);
        } else {
        	StdDraw.setPenColor(StdDraw.BLUE);
           	node.p.drawTo(new Point2D(node.rect.xmax(), node.p.y()));
        	node.p.drawTo(new Point2D(node.rect.xmin(), node.p.y()));
        	
        	drawNode(node.left);
        	drawNode(node.right);
        }
		
	}
	
	public Iterable<Point2D> range(RectHV rect) {    // all points in the set that are inside the rectangle
		Stack<Point2D> stack = new Stack<Point2D>();
		
		rangeNode(root, stack, rect);
		
		return stack;
	}
	
	private void rangeNode(Node2d node, Stack<Point2D> stack, RectHV rect) {
		if (node == null)
			return;
		
		if (!node.rect.intersects(rect))
			return;
		
		if (rect.contains(node.p))
			stack.push(node.p);
		
		rangeNode(node.left, stack, rect);
		rangeNode(node.right, stack, rect);

	}
	
	public Point2D nearest(Point2D p) {               // a nearest neighbor in the set to p; null if set is empty
		Stack<Point2D> stack = new Stack<Point2D>();
		
		nearestNode(root, p, stack, 2.0);
		
		return stack.pop();
	}
	
	private void nearestNode(Node2d node, Point2D queryP, Stack<Point2D> stack, double distance) {
		if (node == null)
			return;
		
		if (distance < node.rect.distanceSquaredTo(queryP))
			return;
		
		if (queryP.distanceSquaredTo(node.p) < distance) {
			stack.push(node.p);
			distance = queryP.distanceSquaredTo(node.p);
		}

		if (node.depth % 2 == 0) {
			if (queryP.x() < node.p.x()) {
				distance = queryP.distanceSquaredTo(stack.peek());
				nearestNode(node.left, queryP, stack, distance);
				distance = queryP.distanceSquaredTo(stack.peek());
				nearestNode(node.right, queryP, stack, distance);
			}
			else {
				distance = queryP.distanceSquaredTo(stack.peek());
				nearestNode(node.right, queryP, stack, distance);
				distance = queryP.distanceSquaredTo(stack.peek());
				nearestNode(node.left, queryP, stack, distance);
			}
		} else {
			if (queryP.y() < node.p.y()) {
				distance = queryP.distanceSquaredTo(stack.peek());
				nearestNode(node.left, queryP, stack, distance);
				distance = queryP.distanceSquaredTo(stack.peek());
				nearestNode(node.right, queryP, stack, distance);
			}
			else {
				distance = queryP.distanceSquaredTo(stack.peek());
				nearestNode(node.right, queryP, stack, distance);
				distance = queryP.distanceSquaredTo(stack.peek());
				nearestNode(node.left, queryP, stack, distance);
			}
		}
	}
   
	/**
	 * @param args
	 */
	public static void main(String[] args) {
		KdTree tree = new KdTree();
		
		Point2D p1 = new Point2D(0.5, 0.5);
		Point2D p2 = new Point2D(0.25, 0.5);
		Point2D p3 = new Point2D(0.75, 0.5);
		
		tree.insert(p1);
		tree.insert(p2);
		tree.insert(p3);

		Point2D qp = new Point2D(0.5, 0.49);
		Point2D np = tree.nearest(qp);

		StdOut.println(np.x() + ", " + np.y());
		
	}

}
